#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <math.h>
#include "ros/ros.h"
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>

typedef const boost::function<void (const nav_msgs::Odometry::ConstPtr& msg)> InsCallback;

static ros::Time time_last = ros::Time(0);
static double vel_last = 0;
static double K = -0.53;
static double T = 0.46;

void msgCallback(const nav_msgs::Odometry::ConstPtr& msg, ros::Publisher &pub)
{
    double vel = msg->twist.twist.angular.z; // Unit is rad.
    ros::Time time = ros::Time::now();
    if(ros::Time(0)==time_last){
        time_last = time;
        vel_last = vel;
        return;
    }
    ros::Duration d = time - time_last;
    double delta_t = d.toSec();
    if( fabs(delta_t)>1e-2){
        double acc = (vel-vel_last)/delta_t;
        std_msgs::Float64 rudder_msg;
        rudder_msg.data = (T*acc+vel)/K;
        time_last = time;
        vel_last = vel;
        pub.publish(rudder_msg);
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv, "test_nomoto_model");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Float64>("rudder", 100);
    InsCallback msg_handle= boost::bind(msgCallback, _1, boost::ref(pub));
    ros::Subscriber sub = n.subscribe("ang_vel", 100, msg_handle);
    ros::spin();

    return 0;
}
